#include <ros/ros.h>
#include "math.h"
#include "std_msgs/String.h"

#include <SLAM/Location.h>
#include <driver/Movement.h>
#include <turtlesim/TeleportAbsolute.h>
#include <filter/filter.h>
#include <SLAM/SwitchPhase.h>

/**
 * 	The tracker class is designed to track and visualize the position
 * 	of a robot, generated from the encoder output values supplied by
 * 	a differential-drive motor system.
 **/
class Tracker
{
	ros::Subscriber    	movement_subscriber;	// 	Subscribe to movement updates.
	ros::ServiceServer 	location_server;	//	Publish encoder -> displacement service.
	ros::Subscriber phase_subscriber_;

public:
			Tracker 			();
	bool 	location_service	(SLAM::Location::Request 			& req,
					 	 	 	 SLAM::Location::Response 			& res);
	void 	movement_callback 	(const driver::Movement::ConstPtr 	& msg);
	void 	switch_phase_callback(const SLAM::SwitchPhase::ConstPtr& msg);

private:
	Filter 	filter_;
	double	wheel_base;
  	float 	current_angle;		//	Current angle relative to start.
	float 	current_x; 			//	Current x position.
	float 	current_y;			//	Current y position.
};
